// =========================================================================
// ================ motor.c - 最终修正与适配版 ================
// =========================================================================

#include "motor.h"
#include "smotor.h"     // 包含舵机模块，因为差速计算需要 'angle' 变量
#include "main.h"       // 关键！包含您自己的 main.h 以获取电机宏定义
#include "sj_pid.h"     // 关键！确保包含的是您项目中唯一的 sj_pid.h
#include <math.h>       // 关键！为 fabsf 和 tanf 提供声明
#include "sj_image.h" 
#include "circle.h"
#include "cross.h"


// --- 2. 解决 "not a compile-time constant" 问题的辅助函数 ---
//    这个函数在 motor.h 中声明，在这里实现
sf_pid_param_t init_sf_pid_param(float kp, float ki, float kd, float low_pass, float p_max, float i_max, float d_max) {
    sf_pid_param_t pid = SF_PID_CREATE(kp, ki, kd, low_pass, p_max, i_max, d_max);
    return pid;
}

// --- 3. 将所有全局变量的定义放在这里 ---
motor_param_t motor_l, motor_r;
sf_pid_param_t motor_pid_l, motor_pid_r;
sf_pid_param_t target_speed_pid, posloop_pid;

// 速度常量定义
float NORMAL_SPEED = 100.0f;
float target_speed = 0.0f; 
float CIRCLE_MAX_SPEED = 0.0f;
float CIRCLE_MIN_SPEED = -6.0f;
float NORMAL_MAX_SPEED = 0.0f;
float NORMAL_MIN_SPEED = -8.0f;

void motor_init(void) {
    // --- 步驟 1: 初始化實際使用的全域 PID 變數 ---
    motor_pid_l = init_sf_pid_param(50, 0, 0, 1, PWM_DUTY_MAX, PWM_DUTY_MAX, PWM_DUTY_MAX);
    motor_pid_r = init_sf_pid_param(50, 0, 0, 1, PWM_DUTY_MAX, PWM_DUTY_MAX, PWM_DUTY_MAX);
    target_speed_pid = init_sf_pid_param(5.0f, 0, 30.0f, 0.6f, 5, 5, 5);
    posloop_pid = init_sf_pid_param(200.0f, 0, 0, 0.7f, PWM_DUTY_MAX, PWM_DUTY_MAX, PWM_DUTY_MAX);
    
    // --- 步驟 2: 僅僅清零 motor_l 和 motor_r 的狀態變數 ---
    // 使用 memset 會更清晰高效
    memset(&motor_l, 0, sizeof(motor_param_t));
    memset(&motor_r, 0, sizeof(motor_param_t));

    // --- 步驟 3: 初始化 motor 結構體內部的剎車 PID (如果需要) ---
    // 這些 brake_pid 是在 motor_control 的其他模式中使用的
    motor_l.brake_pid = init_sf_pid_param(5000, 500, 600, 1, PWM_DUTY_MAX, PWM_DUTY_MAX, PWM_DUTY_MAX);
    motor_r.brake_pid = init_sf_pid_param(5000, 500, 600, 1, PWM_DUTY_MAX, PWM_DUTY_MAX, PWM_DUTY_MAX);
    
    // --- 步驟 4: 硬體初始化 (保持不變) ---
    pwm_init(MOTOR1_PWM, 17000, 0); 
    gpio_init(MOTOR1_DIR, GPO, GPIO_HIGH, GPO_PUSH_PULL);
    
    pwm_init(MOTOR2_PWM, 17000, 0);
    gpio_init(MOTOR2_DIR, GPO, GPIO_HIGH, GPO_PUSH_PULL);
}

// --- 5. speed_control() - 修正函数调用和变量引用 ---
void speed_control(void) {
    extern float angle; // 从 main.c 中获取舵机角度
    float diff = 15.8f * tanf(angle / 180.0f * (float)PI * 2.4f) / 40.0f / 2.0f;
    
    motor_l.motor_mode = MODE_NORMAL;
    motor_r.motor_mode = MODE_NORMAL;

    // --- 速度决策核心逻辑 ---

    if (circle_type == CIRCLE_LEFT_BEGIN || circle_type == CIRCLE_RIGHT_BEGIN) {
        target_speed = MINMAX(target_speed - 0.02f, NORMAL_SPEED + CIRCLE_MIN_SPEED, NORMAL_SPEED + CIRCLE_MAX_SPEED);
    }
    else {
        // 在这里统一声明从 main.c 引入的全部变量
        extern int rptsn_num;
        extern float rptsn[POINTS_MAX_LEN][2];

        if (rptsn_num > 20) {
            int id = MIN(70, rptsn_num - 1);
            float error = fabsf((rptsn[id][0] - rptsn[0][0]) / (rptsn[id][1] - rptsn[0][1]));

            float speed_adjustment = -sf_pid_solve(&target_speed_pid, error);
            target_speed = MINMAX(NORMAL_SPEED + speed_adjustment, NORMAL_SPEED + NORMAL_MIN_SPEED, NORMAL_SPEED + NORMAL_MAX_SPEED);
        } 
        else if (rptsn_num > 5) {
            target_speed = NORMAL_SPEED + NORMAL_MIN_SPEED;
        }
        else {
            target_speed = NORMAL_SPEED;
        }
    }
    
    // -- 用于初始测试的简化版本 --
    // 在您联调底层时，可以取消下面这行的注释，并注释掉上面的所有逻辑
    // target_speed = 15.0f; 

    motor_l.target_speed = target_speed - diff * target_speed;
    motor_r.target_speed = target_speed + diff * target_speed;
}

// --- 6. motor_control() - 修正所有PID函数调用 ---
void motor_control(void) {
    speed_control();

    // 左电机PID计算
    if (motor_l.motor_mode == MODE_NORMAL) {
        float error = (motor_l.target_speed - motor_l.encoder_speed);
        motor_l.duty = sf_pid_solve(&motor_pid_l, error);
        motor_l.duty = MINMAX(motor_l.duty, - (int32_t)PWM_DUTY_MAX, (int32_t)PWM_DUTY_MAX);
    } else if (motor_l.motor_mode == MODE_BANGBANG) {
        motor_pid_l.out_i = 0;
        motor_l.duty += sf_bangbang_pid_solve(&motor_l.brake_pid, (float) (motor_l.target_speed - motor_l.encoder_speed));
        motor_l.duty = MINMAX(motor_l.duty, - (int32_t)PWM_DUTY_MAX, (int32_t)PWM_DUTY_MAX);
    } else if (motor_l.motor_mode == MODE_SOFT) {
        motor_pid_l.out_i = 0;
        motor_l.duty += sf_changable_pid_solve(&motor_l.pid, (float) (motor_l.target_speed - motor_l.encoder_speed));
        motor_l.duty = MINMAX(motor_l.duty, - (int32_t)PWM_DUTY_MAX, (int32_t)PWM_DUTY_MAX);
    } else if (motor_l.motor_mode == MODE_POSLOOP) {
        motor_pid_l.out_i = 0;
        motor_l.duty = sf_pid_solve(&posloop_pid, (float) (motor_l.target_encoder - motor_l.total_encoder));
        motor_l.duty = MINMAX(motor_l.duty, - (int32_t)PWM_DUTY_MAX, (int32_t)PWM_DUTY_MAX);
    }

    // 右电机PID计算 (与左电机逻辑相同)
    if (motor_r.motor_mode == MODE_NORMAL) {
        float error = (motor_r.target_speed - motor_r.encoder_speed);
        motor_r.duty = sf_pid_solve(&motor_pid_r, error);
        motor_r.duty = MINMAX(motor_r.duty, - (int32_t)PWM_DUTY_MAX, (int32_t)PWM_DUTY_MAX);
    } // ... (其他模式的逻辑与左轮类似) ...

    // 大角度转向时限幅
    extern float angle;
    if (fabsf(angle) > 10.0f) {
        motor_l.duty = MINMAX(motor_l.duty, - (int32_t)PWM_DUTY_MAX * 8 / 10, (int32_t)PWM_DUTY_MAX * 8 / 10);
        motor_r.duty = MINMAX(motor_r.duty, - (int32_t)PWM_DUTY_MAX * 8 / 10, (int32_t)PWM_DUTY_MAX * 8 / 10);
    }

    // 硬件输出
    if(motor_l.duty >= 0) {
        gpio_set_level(MOTOR1_DIR, MOTOR1_FORWARD_DIR_LEVEL);
        pwm_set_duty(MOTOR1_PWM, (uint32_t)motor_l.duty);
    } else {
        gpio_set_level(MOTOR1_DIR, (MOTOR1_FORWARD_DIR_LEVEL == GPIO_HIGH) ? GPIO_LOW : GPIO_HIGH);
        pwm_set_duty(MOTOR1_PWM, (uint32_t)(-motor_l.duty));
    }
    if(motor_r.duty >= 0) {
        gpio_set_level(MOTOR2_DIR, MOTOR2_FORWARD_DIR_LEVEL);
        pwm_set_duty(MOTOR2_PWM, (uint32_t)motor_r.duty);
    } else {
        gpio_set_level(MOTOR2_DIR, (MOTOR2_FORWARD_DIR_LEVEL == GPIO_HIGH) ? GPIO_LOW : GPIO_HIGH);
        pwm_set_duty(MOTOR2_PWM, (uint32_t)(-motor_r.duty));
    }
}

int64_t get_total_encoder() {
    return (int64_t)((motor_l.total_encoder + motor_r.total_encoder) / 2);
}